// -*- C++ -*-
/*!
 * @file  MonteCarloLocalizationLRF.cpp
 * @brief Monte Carlo Localization using LRF
 * @date $Date$
 *
 * $Id$
 */

#include "MonteCarloLocalizationLRF.h"
#include "mcl.h"
#include "map.h"
#include "const.h"

// GTK
#ifndef _WITHOUT_GUI
 #include "interface.h"
 #include "support.h"
 #include "draw.h"
#endif



#ifndef M_PI
#define M_PI		3.14159265358979
#endif

#define M_PI2		M_PI * 2
#define M_PI_div2	M_PI/2.0


// Module specification
// <rtc-template block="module_spec">
static const char* montecarlolocalizationlrf_spec[] =
  {
    "implementation_id", "MonteCarloLocalizationLRF",
    "type_name",         "MonteCarloLocalizationLRF",
    "description",       "Monte Carlo Localization using LRF",
    "version",           "0.8.1",
    "vendor",            "Tsuyoshi Suenaga, NAIST",
    "category",          "localization",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default._num_particles", "300",
    "conf.default._def_pos_x_m", "3.4",
    "conf.default._def_pos_y_m", "7.7",
    "conf.default._def_pos_theta_deg", "180.0",
    "conf.default._std_pos_x_m", "0.5",
    "conf.default._std_pos_y_m", "0.5",
    "conf.default._std_pos_theta_deg", "10.0",
    "conf.default.__m_per_pixel", "0.04",
    "conf.default._screen_offset_x", "-200",
    "conf.default._screen_offset_y", "150",
    "conf.default._map_format", "OCCUPANCY_GRID_MAP",
    "conf.default._map_filename", "mapdata/RTCcenterUnifiedMap.pgm",
    "conf.default._x_coord_front_in_range_data", "0",
    "conf.default._x_coord_front_in_current_pose", "1",
    "conf.default._x_coord_front_out_current_pose", "1",
    "conf.default._x_coord_front_in_odometry", "1",
    "conf.default._scanAngle", "4.7124",
    "conf.default._scanStep", "0.004363323",
    "conf.default._offsetfov", "-0.7854",
    ""
  };
// </rtc-template>

MonteCarloLocalizationLRF::MonteCarloLocalizationLRF(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_RangeDataIn("RangeData", m_RangeData),
    m_OdometryIn("Odometry", m_Odometry),
    m_CurrentPosInitIn("CurrentPosInit", m_CurrentPosInit),
    m_CurrentPosOut("CurrentPos", m_CurrentPos),
    m_LocationInfoPort("LocationInfo"),
    m_LRFInfoPort("LRFInfo"),
    m_MapInfoPort("MapInfo"),
    // </rtc-template>
    m_Location(mcl, &pose_mean, &pose_std),
	dummy(0)
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  registerInPort("RangeData", m_RangeDataIn);
  registerInPort("Odometry", m_OdometryIn);
  registerInPort("CurrentPosInit", m_CurrentPosInitIn);
  
  // Set OutPort buffer
  registerOutPort("CurrentPos", m_CurrentPosOut);
  
  // Set service provider to Ports
  m_LocationInfoPort.registerProvider("Location", "LocationInfo", m_Location);
  
  // Set service consumers to Ports
  m_LRFInfoPort.registerConsumer("LRF", "LRFInfo", m_LRF);
  m_MapInfoPort.registerConsumer("Map", "MapInfo", m_Map);
  
  // Set CORBA Service Ports
  registerPort(m_LRFInfoPort);
  registerPort(m_MapInfoPort);
  registerPort(m_LocationInfoPort);
  
  // </rtc-template>

// GTK関連

#ifndef _WITHOUT_GUI
  initialize_for_gtk();
#endif

}

MonteCarloLocalizationLRF::~MonteCarloLocalizationLRF()
{
}



RTC::ReturnCode_t MonteCarloLocalizationLRF::onInitialize()
{
  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("_num_particles", m__num_particles, "300");
  bindParameter("_def_pos_x_m", m__def_pos_x_m, "3.4");
  bindParameter("_def_pos_y_m", m__def_pos_y_m, "7.7");
  bindParameter("_def_pos_theta_deg", m__def_pos_theta_deg, "180.0");
  bindParameter("_std_pos_x_m", m__std_pos_x_m, "0.5");
  bindParameter("_std_pos_y_m", m__std_pos_y_m, "0.5");
  bindParameter("_std_pos_theta_deg", m__std_pos_theta_deg, "10.0");
  bindParameter("_m_per_pixel", m__m_per_pixel, "0.04");
  bindParameter("_screen_offset_x", m__screen_offset_x, "-200");
  bindParameter("_screen_offset_y", m__screen_offset_y, "150");
  bindParameter("_map_format", m__map_format, "OCCUPANCY_GRID_MAP");
  bindParameter("_map_filename", m__map_filename, "mapdata/RTCcenterUnifiedMap.pgm");
  //  bindParameter("_map_filename", m__map_filename, "mapdata/sample1.pgm");
  bindParameter("_x_coord_front_in_range_data", m__x_coord_front_lrf_info, "0");
  bindParameter("_x_coord_front_in_current_pose", m__x_coord_front_current_pos_init, "1");
  bindParameter("_x_coord_front_out_current_pose", m__x_coord_front_current_pos, "1");
  bindParameter("_x_coord_front_in_odometry", m__x_coord_front_odometry, "1");
  bindParameter("_scanAngle", m__scanAngle, "4.7124");
  bindParameter("_scanStep", m__scanStep, "0.004363323");
  bindParameter("_offsetfov", m__offsetfov, "-0.7854");
//  bindParameter("_offsetfov", m__offsetfov, "0.00");
  // </rtc-template>

  mcl = new Mcl;
  mcl->filter = NULL;

  return RTC::RTC_OK;
}


RTC::ReturnCode_t MonteCarloLocalizationLRF::onFinalize()
{
  delete mcl;
  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t MonteCarloLocalizationLRF::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MonteCarloLocalizationLRF::onShutdown(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/


RTC::ReturnCode_t MonteCarloLocalizationLRF::onActivated(RTC::UniqueId ec_id)
{
 
  PortServiceList* portlist;

  findConnectionToMapInfo =  findConnectionToOdometry = false;
  std::cerr << "[Info] onActivated " << std::endl;

  /// Check MapInfo port and Odometry port connection
  portlist = this->get_ports();
  port_connect_check = new bool [portlist->length()];

  for(unsigned int i=0; i<portlist->length(); i++){
    PortService_ptr port;
    port = (*portlist)[i];
	std::string name = std::string(port->get_port_profile()->name);
	int connect_len = port->get_port_profile()->connector_profiles.length();
	//std::cout << "port[" << i << "]=" << port->get_port_profile()->name << std::endl;

	if(name.find("MapInfo", 0) != std::string::npos && connect_len != 0){
        findConnectionToMapInfo = true;
	}else if(name.find("Odometry", 0) != std::string::npos && connect_len != 0){
        findConnectionToOdometry = true;
    }
  }


  std::cerr << "LRF Information : front coordinate is ";
  if(m__x_coord_front_lrf_info){
    std::cerr << "X.";
  }else{
    std::cerr << "Y.";
  }
  std::cerr << std::endl;

  std::cerr << "Input Current Pos : front coordinate is ";
  if(m__x_coord_front_current_pos_init){
    std::cerr << "X.";
  }else{
    std::cerr << "Y.";
  }
  std::cerr << std::endl;

  std::cerr << "Output Current Pos : front coordinate is ";
  if(m__x_coord_front_current_pos){
    std::cerr << "X.";
  }else{
    std::cerr << "Y.";
  }

  std::cerr << "Input Odometry : front coordinate is ";
  if(m__x_coord_front_odometry){
    std::cerr << "X.";
  }else{
    std::cerr << "Y.";
  }
  std::cerr << std::endl;

// GTK
#ifndef _WITHOUT_GUI
  // Create window
  //window1 = create_window1 ();
  gui = create_window1 ();
  // stert gtk event loop
  g_thread_create((GThreadFunc)gtk_main_thread, NULL, FALSE, NULL);
  initialize_for_gui(gui);

  // set offsets
  gui->screen_offset_x = m__screen_offset_x;
  gui->screen_offset_y = m__screen_offset_y;

  // set resolutions [m/pixel]
  gui->screen_resolution = m__m_per_pixel;
#endif

  // set information of LRF
  mcl->lrf.total_points = (int)(m__scanAngle/m__scanStep);
  mcl->lrf.resolution = m__scanStep;

  if(m__x_coord_front_lrf_info){  // X-axis -> Y-axis
    mcl->lrf.offset_fov = m__offsetfov+M_PI/2.0;
  }else{  // Y-axis
    mcl->lrf.offset_fov = m__offsetfov;
  }
  
  mcl->lrf.fov = (mcl->lrf.total_points - 1) * mcl->lrf.resolution;
  mcl->lrf.range.resize(mcl->lrf.total_points);
    
  std::cerr << "TotalPoints: " << mcl->lrf.total_points << std::endl;
  std::cerr << "Resolution : " << mcl->lrf.resolution << std::endl;
  std::cerr << "OffsetFOV  : " << mcl->lrf.offset_fov << std::endl;
  std::cerr << "FieldOfView: " << mcl->lrf.fov << std::endl;


  // create particles
  mcl->filter = localizer_particle_filter_new(m__num_particles);
  mcl->prev_filter = localizer_particle_filter_new(m__num_particles);

  // Initial position，mean of posture，standard deviations of posture [m] or [rad]
  pose_mean.x = m__def_pos_x_m;
  pose_mean.y = m__def_pos_y_m;
  pose_mean.theta = DEG2RAD(m__def_pos_theta_deg);
  pose_std.x = m__std_pos_x_m;
  pose_std.y = m__std_pos_y_m;
  pose_std.theta = DEG2RAD(m__std_pos_theta_deg);
  
  // Initialize paticle
  localizer_initialize_particles_gaussians(mcl->filter, 1, &pose_mean, &pose_std);

  // Initialize odometry
#if 1
  mcl->odometry.x =  mcl->prev_odometry.x = 3.24;
  mcl->odometry.y =  mcl->prev_odometry.y = 7.81;
  mcl->odometry.theta = mcl->prev_odometry.theta = -1.57;

#else
  mcl->odometry.x = 0;
  mcl->odometry.y = 0;
  mcl->odometry.theta = 0;
  mcl->prev_odometry.x = 0;
  mcl->prev_odometry.y = 0;
  mcl->prev_odometry.theta = 0;
#endif

  if(findConnectionToMapInfo){  // check  sixth port?  Service port connected with MapManager 
    RTC::TimedOctetSeq mapdata;
    CORBA::Short x, y, width, height;
    CORBA::Double resolution;
	bool res_map = false;
	int count = 0;

	while(count < 10){
	  try{
		res_map = m_Map->getFullGridMap(mapdata, x, y, width, height, resolution);
	  }catch(...){
		std::cerr << "ERROR on call m_Map->getFullGridMap, slepp one second." << std::endl;
		res_map = false;
		sleep(1000);
	  }
	  count++;
	}
    if(res_map != false){
      mcl->ogmap.origin_x = x;
      mcl->ogmap.origin_y = y;
      mcl->ogmap.width  = width;
      mcl->ogmap.height = height;
      mcl->ogmap.resolution = resolution;

      for(int i=0;i<width*height;i++){
		 mcl->ogmap.data[i] = mapdata.data[i];
	  }
      std::cout << "Received Grid Map: " << std::endl;
      std::cout << "  (x,y)=(" << x << "," << y << ")" << std::endl;
      std::cout << "  (w,h)=(" << width << "," << height << ")" << std::endl;
      std::cout << "  resolution=" << resolution << std::endl;
    }else{
      std::cout << "Could not receive grid map." << std::endl;
      return RTC::RTC_ERROR;
    }
    convert_occupancy_gridmap_to_pointmap(&mcl->ogmap, &mcl->pmap, 200);

  }else{
	// Load map files from local storage.
    // Read map
    if(!m__map_format.find("POINT_MAP")){
      //  Point map // [cm] -> [m]
      //load_point_map("mapdata/pointmap_5f_cm.dat", &mcl->pmap, 0.01); 
      load_point_map(m__map_filename.c_str(), &mcl->pmap, 0.01);

    }else if(!m__map_format.find("LINE_MAP")){
      // Line map  [cm] -> [m]  and  [m/grid]
      //load_line_map("mapdata/linemap_5f_cm.dat", &mcl->lmap, 0.01); 
      load_line_map(m__map_filename.c_str(), &mcl->lmap, 0.01);
      convert_linemap_to_pointmap(&mcl->lmap ,&mcl->pmap, 0.07);

    }else if(!m__map_format.find("OCCUPANCY_GRID_MAP")){
      // Occuancy grid map
      //load_occupancy_grid_map("mapdata/NAIST1F-occupancyGridMap.pgm", &mcl->ogmap);
      load_occupancy_grid_map(m__map_filename.c_str(), &mcl->ogmap);
      convert_occupancy_gridmap_to_pointmap(&mcl->ogmap, &mcl->pmap, 200);

    }else{
      return RTC::RTC_ERROR;
    }

  }


#ifdef CV_DISP

  	imOrgmap = cvCreateImage( cvSize( 200,100 ), IPL_DEPTH_8U, 3 );
	imTrjmap = cvCloneImage(imOrgmap);
	imRobmap = cvCloneImage(imOrgmap);

	cvNamedWindow("MapImage",CV_WINDOW_AUTOSIZE);

#endif

  return RTC::RTC_OK;
}



RTC::ReturnCode_t MonteCarloLocalizationLRF::onDeactivated(RTC::UniqueId ec_id)
{
  localizer_particle_filter_delete(mcl->filter);
  localizer_particle_filter_delete(mcl->prev_filter);

  	while(!m_OdometryIn.isEmpty()){
		m_OdometryIn.read();
	}
	while(!m_CurrentPosInitIn.isEmpty()){
		m_CurrentPosInitIn.read();
	}
    while(!m_RangeDataIn.isEmpty()){
		  m_RangeDataIn.read();
	}
#ifndef _WITHOUT_GUI
  // close windows
  destroy_window(gui);
  // quit the gtk event loop
  gtk_main_quit();
#endif

  return RTC::RTC_OK;
}


double normalize_theta(double th){
  double res = th;

  while(res < -M_PI || res > M_PI){
	  if(res < -M_PI){
		  res += M_PI2;
	  }else if(res > M_PI){
		   res -= M_PI2;
	  }
  }
  return res;
}

RTC::ReturnCode_t MonteCarloLocalizationLRF::onExecute(RTC::UniqueId ec_id)
{
  int i;
  double likelihood;

  //std::cerr << "[Info] onExecute " << std::endl;

  // Arrive odometry data
  if(m_OdometryIn.isNew()){
	while(!m_OdometryIn.isEmpty()){
		m_OdometryIn.read();
	}

	/// conversions
	
	/*
    if(m__x_coord_front_odometry){  
      // X-axis -> Y-axis
      mcl->odometry.x = -m_Odometry.data.position.y;
      mcl->odometry.y = m_Odometry.data.position.x;
    }else{
      // Y-axis
      mcl->odometry.x = m_Odometry.data.position.x;
      mcl->odometry.y = m_Odometry.data.position.y;
    }
    mcl->odometry.theta = m_Odometry.data.heading;
	*/

	mcl->odometry.x = m_Odometry.data.position.x;
    mcl->odometry.y = m_Odometry.data.position.y;


	if(m__x_coord_front_odometry){  
      mcl->odometry.theta = m_Odometry.data.heading - M_PI_div2;
    }else{
      mcl->odometry.theta = m_Odometry.data.heading;
    }


    //std::cerr << "m_OdmetryIn " << std::endl;
  }

  // Arrive currnet position data

  if(m_CurrentPosInitIn.isNew()){
    // read current position and initialize
	while(!m_CurrentPosInitIn.isEmpty()){
		m_CurrentPosInitIn.read();
	}
    m_CurrentPosInitIn.read();
    pose_mean.x = m_CurrentPosInit.data.position.x;
    pose_mean.y = m_CurrentPosInit.data.position.y;

    if(m__x_coord_front_current_pos_init){
      // X-axis -> Y-axis
      pose_mean.theta = m_CurrentPosInit.data.heading - M_PI_div2;
    }else{
      // Y-axis
      pose_mean.theta = m_CurrentPosInit.data.heading;
    }
    pose_std.x = m__std_pos_x_m;
    pose_std.y = m__std_pos_y_m;
    pose_std.theta = DEG2RAD(m__std_pos_theta_deg);
    
    // Initalize particles
    localizer_initialize_particles_gaussians(mcl->filter, 1, &pose_mean, &pose_std);
  }

  //  Arrive range data
  if(m_RangeDataIn.isNew()){	
	  while(!m_RangeDataIn.isEmpty()){
		  m_RangeDataIn.read();
	  }

    // check length of data
    if(!m_RangeData.data.length()){
      std::cerr<<"Receive data length " <<m_RangeData.data.length() << std::endl;

    }else{

	  for(i=0; i<mcl->lrf.total_points &&  i<mcl->lrf.range.size(); i++){
        mcl->lrf.range.at(i) = m_RangeData.data[i]/1000.0;
      }

      // MCL
      if(findConnectionToOdometry == false){
        localizer_particle_resample(mcl->filter);
      }else{
        sample_motion_model_odometry(mcl->filter, &mcl->odometry, &mcl->prev_odometry);
      }

      mcl->filter->sum_weight = 0;
      mcl->filter->max_particle.weight = 0;

      for( i=0 ; i<mcl->filter->num_particles; i++){
        if(findConnectionToMapInfo || !m__map_format.find("OCCUPANCY_GRID_MAP")){ 

			likelihood = likelihood_field_range_finder_model( &mcl->lrf,
							  mcl->filter->particles[i],
							  &mcl->ogmap);
        }else{
			likelihood = likelihood_field_range_finder_model( &mcl->lrf,
							  mcl->filter->particles[i],
							  &mcl->pmap);
        }

        mcl->filter->sum_weight += likelihood;
        mcl->filter->particles[i].weight = likelihood;

        if ( likelihood > mcl->filter->max_particle.weight ){
		  mcl->filter->max_particle = mcl->filter->particles[i];
        }
      }

	  qsort( mcl->filter->particles,  mcl->filter->num_particles,
			sizeof(LocalizerParticle), compare_particle);

	  int  n = 0;
	  double sx, sy,stheta;
	  sx=sy=stheta=0.0;

	  for( i = 0 ; i < 20 && mcl->filter->particles[i].weight > 0.1 ; i++){
        sx += mcl->filter->particles[i].x;
		sy += mcl->filter->particles[i].y;
		mcl->filter->particles[i].theta = normalize_theta(mcl->filter->particles[i].theta);
		stheta += mcl->filter->particles[i].theta;
		n++;
      }

      // normalize particle
      for( i = 0 ; i < mcl->filter->num_particles; i++){
        mcl->filter->particles[i].weight /= mcl->filter->sum_weight;
      }

      mcl->filter->sum_weight = 1.0;
      mcl->filter->ready= 1;
      memcpy(mcl->prev_filter->particles, mcl->filter->particles,  sizeof(LocalizerParticle)*mcl->filter->num_particles);
/*
      std::cout << "(x,y,theta) = (" \
	      << mcl->filter->max_particle.x << ", " \
	      << mcl->filter->max_particle.y << ", " \
	      << RAD2DEG(mcl->filter->max_particle.theta) << ")  " \
	      << mcl->filter->max_particle.weight << std::endl;
*/
      // output current data
	  /*
      m_CurrentPos.data.position.x = mcl->filter->max_particle.x;
      m_CurrentPos.data.position.y = mcl->filter->max_particle.y;


      if(m__x_coord_front_current_pos){
        // Y-axis -> X-axis ?
        m_CurrentPos.data.heading = mcl->filter->max_particle.theta + M_PI/2.0;
      }else{
        // Y-axis
        m_CurrentPos.data.heading = mcl->filter->max_particle.theta;
      }
	  */

	  if (n> 0){
		  /*
	    m_CurrentPos.data.position.x = sx/n ;
        m_CurrentPos.data.position.y = sy/n ;
	    m_CurrentPos.data.heading = stheta/n;
			*/
	    m_CurrentPos.data.position.x = mcl->filter->particles[0].x ;
        m_CurrentPos.data.position.y = mcl->filter->particles[0].y ;
	    m_CurrentPos.data.heading = mcl->filter->particles[0].theta;


	    if(m__x_coord_front_current_pos){
          m_CurrentPos.data.heading += M_PI_div2;
        }
		m_CurrentPos.data.heading = normalize_theta( m_CurrentPos.data.heading );

	    std::cout << n << " (x,y,theta) = (" \
	      <<  m_CurrentPos.data.position.x << ", " \
	      <<  m_CurrentPos.data.position.y << ", " \
	      << RAD2DEG( m_CurrentPos.data.heading ) << ")  " \
	      << std::endl;

		m_CurrentPos.tm.sec  = m_RangeData.tm.sec;
		m_CurrentPos.tm.nsec = m_RangeData.tm.nsec;

        m_CurrentPosOut.write();
	  }
	}
  }


// GTK
#ifndef _WITHOUT_GUI
  gdk_threads_enter();
  
  // initalize a pixmap
  draw_area_initialize(gui);
  
  // draw map on the pixmap
  draw_pointmap(&mcl->pmap, gui);
  
  if(mcl->filter != NULL) {
    // draw particles
    draw_particles( gui,
		    mcl->filter->particles, 
		    mcl->filter->num_particles,
		    10);
    // draw center of sensor on the pixmap
    // draw_point(gui, &pose_mean, 4, make_color(0x9999, 0x9999, 0x9999));
    draw_particle(gui, &mcl->filter->max_particle, 10,
		  make_color(0xfb00,0xe700,0x1800),
		  make_color(0xff00,0x0,0x0));
    // draw LRF data on the pixmap
    draw_laser_range(gui, &mcl->lrf, &mcl->filter->max_particle);
  }
  // draw the pixmap
  show_draw_area(gui);

  // get size of window 
  get_draw_size(gui, &width, &height);
//   fprintf(stderr, "(%4d,%4d)\n",
// 	  width, height);
  gdk_threads_leave();

#endif
  
  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t MonteCarloLocalizationLRF::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MonteCarloLocalizationLRF::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MonteCarloLocalizationLRF::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MonteCarloLocalizationLRF::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t MonteCarloLocalizationLRF::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/



extern "C"
{
 
  void MonteCarloLocalizationLRFInit(RTC::Manager* manager)
  {
    RTC::Properties profile(montecarlolocalizationlrf_spec);
    manager->registerFactory(profile,
                             RTC::Create<MonteCarloLocalizationLRF>,
                             RTC::Delete<MonteCarloLocalizationLRF>);
  }
  
};


